/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/TrajSrc/bezier_traj.cpp
 * @Description  : 实现了贝塞尔轨迹的生成和管理，包括初始化、进度设置、姿态投影等功能。 
 *                 主要用于AGV路径规划，支持粒子群优化算法来计算最佳路径参数。 
 *                 代码中定义了贝塞尔曲线的创建方法、车辆的运动模式、速度生成函数和舵角生成函数等。 
 *                 通过对全局变量的管理，提供了对车辆运动状态的实时监控和调整。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:25:57
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include <string>
#include "bezier_traj.h"

constexpr float AGV_CTRL_CYCLE = 0.05f; // agv控制周期
constexpr int PSO_FIND_POS_VEL = 5;     // 第一次寻找位姿时的速度,加快迭代搜索
constexpr int NUM_PARTICLES = 12;       // 粒子数量
constexpr int NUM_VARIABLES = 1;        // 变量数量 
constexpr int MAX_ITERATION = 100;      // 最大迭代数

Bezier* global_bezier = nullptr; // 全局贝塞尔曲线指针
float global_prog_rate = 0;      // 全局当前运行的速度
Posture global_posture;          // 全局位姿

//
// set value by fGlobalProgRate
// 根据当前设定的速度,计算当前评估位置的值
//
static void ObjFunByProg(Particle* particle)
{
    if (!particle || !global_bezier) return;

    const double* position = particle->GetPosition();
    if (Particle::GetNumberOfVariables() != NUM_VARIABLES || !position) {
        particle->SetError(PARTICLE_VARIABLE_NOT_MATCH);
        return;
    }

    // ComputeRelativeLen:pPosition[0] is T
    // The smller to the theoretical curve length value, the smaller the value, and the more accurate t is
    // 根据当前的位置; 评估位置好坏的量值
    // 优化目标函数
    float value = global_bezier->ComputeRelativeLength(position[0]) - global_prog_rate * AGV_CTRL_CYCLE;
    particle->SetValue(fabs(value));

    // Add constraint conditions: kinematic constraints need to be satisfied,
    // such as angular velocity, angular acceleration and velocity acceleration
    // pPosition[0] > 1; T max is 1
    // 如果位置超限,设置评估值为无限大;设置可行性
    double constraints = position[0] > 1 ? 10000000 * (position[0]) : 0.0;
    particle->SetConstraints(constraints);
    particle->SetFeasible((fabs(constraints) > 1e-6f) ? false : true);
}

//
// set value by GlobalPosture
// 根据全局位姿评估当前位置的好坏
//
static void ObjFunByPosture(Particle *particle)
{
    if (!particle || !global_bezier) return;

    const double* position = particle->GetPosition();
    if (Particle::GetNumberOfVariables() != NUM_VARIABLES || !position) {
        particle->SetError(PARTICLE_VARIABLE_NOT_MATCH);
        return;
    }

    // ComputeRefPoint;
    // 设置当前评估位置好坏的量值
    // 优化目标函数
    float value = global_bezier->ComputeReferencePoint(position[0], global_posture.x_, global_posture.y_);
    particle->SetValue(fabs(value));

    double constraints = position[0] > 1 ? 10000000 * (position[0]) : 0.0;
    particle->SetConstraints(constraints);
    particle->SetFeasible((fabs(constraints) > 1e-6f) ? false : true);
}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CBezierTraj".
BezierTrajectory::BezierTrajectory() : bezier_(nullptr) 
{
}

BezierTrajectory::~BezierTrajectory() 
{
    if (bezier_ != nullptr) {
        delete bezier_;
        bezier_ = nullptr;
    }
}

//
//   Do initializations that are specific to the Bezier trajectory.
//
void BezierTrajectory::CreateBezier(Posture& start_posture, Posture& end_posture, float len1, float len2) 
{
    type_ = (short)TrajType::BEZIER_TRAJ;
    bezier_ = new Bezier(start_posture, end_posture, len1, len2);
    ASSERT(bezier_ != nullptr);
}

//
//   Do initializations that are specific to the Bezier trajectory.
//
void BezierTrajectory::CreateBezier(const Point& start_point, const Point& ctrl_point1, const Point& ctrl_point2, const Point& end_point) 
{
    type_ = (short)TrajType::BEZIER_TRAJ;
    bezier_ = new Bezier(start_point, ctrl_point1, ctrl_point2, end_point);
    ASSERT(bezier_ != nullptr);
}

//
//   Create the Bezier trajectory.
//
void BezierTrajectory::CreateTrajectory(Posture& start_posture, Posture& end_posture, float len1, float len2,
                                  MoveDir wheel_move_dir, bool is_shift, Angle& shift_heading, float from, float to) 
{
    CreateBezier(start_posture, end_posture, len1, len2);
    Init(wheel_move_dir, is_shift, shift_heading, from, to);
}

//
//   Create the Bezier trajectory.
//
void BezierTrajectory::CreateTrajectory(Point& start_point, Point& ctrl_point1, Point& ctrl_point2, Point& end_point,
                                  MoveDir wheel_move_dir, bool is_shift, Angle& shift_heading, float from, float to) 
{
    CreateBezier(start_point, ctrl_point1, ctrl_point2, end_point);
    Init(wheel_move_dir, is_shift, shift_heading, from, to);
}

//
//   Do basic initializations.
//
void BezierTrajectory::Init(MoveDir wheel_move_dir, bool is_shift, Angle& shift_heading, float from, float to) 
{
	// 只有非全方位行走时，采用此轮转向，全方位时，自动计算
    wheel_move_dir_ = wheel_move_dir;
    // 非全方位移动时,舵角默认值
    end_steer_ = Angle(0.0f);
    start_steer_ = end_steer_;
    steer_angle_ = start_steer_;

    is_shift_move_ = is_shift;
    shift_heading_ = shift_heading;

    // 是否开启强制反转舵角
    is_steer_reverse_ = FALSE;

    float len = bezier_->Length();
    if (from > len) from = len;
    if (to < 0 || to > len) to = len;

    from_ = from;
    to_ = to;
    range_ = fabs(to - from);

    // 注意:shiftMove时,完全相同;m_angEndHeading = m_angStartHeading = m_angHeading = m_angShiftHeading
    // #1 计算开始车头方向角和开始曲率、开始舵角
    SetCurrentT(0.0f, from_ / len);
    start_heading_ = heading_angle_;
    start_curvature_ = curvature_;

    if (is_shift_move_) {
        start_steer_ = bezier_->TangentFunction() - heading_angle_;
        DEBUG_OUT("BezierTrajectory init: ShiftMove:start_steer.degree=" << start_steer_.Degree());
    }

    //  #2 计算中间舵角
    if (is_shift_move_) {
        float middle_len = (to_ - from_) / 3.0f;
        SetCurrentT(0.0f, middle_len / len);
        middle_steer_ = bezier_->TangentFunction() - heading_angle_;
    }

    // #3 计算开结束车头方向角和结束曲率、结束舵角
    SetCurrentT(0.0f, to_ / len);
    end_heading_ = heading_angle_;
    end_curvature_ = curvature_;

    if (is_shift_move_) {
        end_steer_ = bezier_->TangentFunction() - heading_angle_;
        DEBUG_OUT("BezierTrajectory init: ShiftMove:end_steer.degree=" << end_steer_.Degree());
    }

    heading_angle_ = start_heading_;
    bezier_->SetCurveLength(from_);
    bezier_->SetCurrentT(from_ / len);
    bezier_->SetLastT(from_ / len);
    bezier_->SetDeltaT(0.0f);

    last_plan_t_ = 0.0f;
    plan_delta_t_ = 0.0f;
    last_ref_t_ = 0.0f;
    ref_delta_t_ = 0.0f;
    reload_area_flag_ = false;
    SetWheelDir();
}

/**
 * @brief 设置车轮移动方向
 *
 * 根据打舵角度计算车轮移动方向，只针对全方位计算。
 *
 * @note 该方法主要用于设置车轮的移动方向，包括前进、后退以及左右移动。
 */void BezierTrajectory::SetWheelDir() 
{
	// 计算打舵角方向
    // 只针对全方位计算
    if (!is_shift_move_) return;

    if (LessDegree(middle_steer_, 90.0f)) {
        if (LessDegree(start_steer_, 110.0f)) {
            wheel_move_dir_ = MoveDirTag::Forward;
            side_move_dir_ = MoveDirTag::Leftward;
            DEBUG_OUT("#1 SetWheelDir OK(LEFTWARD);wheelDir= FORWARD;start_steer(degree)=" << start_steer_.Degree());
        } else {
            DEBUG_ERROR_OUT("#1 error:steer failed: shift move:start_steer(degree)=" << start_steer_.Degree()
                                                                                     << ",end_steer=" << end_steer_.Degree());
        }
    } else if (LessDegree(middle_steer_, 180.0f)) {
        if (LessDegree(start_steer_, 180.0f) && CanSteerReserve(start_steer_)) {
            wheel_move_dir_ = MoveDirTag::Backward;
            start_steer_ = !start_steer_;
            end_steer_ = !end_steer_;
            side_move_dir_ = MoveDirTag::Leftward;
            is_steer_reverse_ = true;
            DEBUG_OUT("#2 SetWheelDir OK(LEFTWARD);wheelDir= MoveDirTag::Backward;start_steer(degree)=" << start_steer_.Degree()
                                                                                           << ",end_steer=" << end_steer_.Degree());
        } else {
            DEBUG_ERROR_OUT("#2 error:steer failed: shift move:start_steer(degree)=" << start_steer_.Degree()
                                                                                     << ",end_steer=" << end_steer_.Degree());
        }
    } else if (LessDegree(middle_steer_, 270.0f)) {
        if (LessDegree(start_steer_, 270.0f) && CanSteerReserve(start_steer_)) {
            wheel_move_dir_ = MoveDirTag::Backward;
            start_steer_ = !start_steer_;
            end_steer_ = !end_steer_;
            side_move_dir_ = MoveDirTag::Rightward;
            is_steer_reverse_ = true;
            DEBUG_OUT("#3 SetWheelDir OK(RIGHTWARD);wheelDir= MoveDirTag::Backward;start_steer(degree)=" << start_steer_.Degree()
                                                                                             << ",end_steer=" << end_steer_.Degree());
        } else {
            DEBUG_ERROR_OUT("#3 error:steer failed: shift move:start_steer(degree)=" << start_steer_.Degree()
                                                                                     << ",end_steer=" << end_steer_.Degree());
        }
    } else {
        wheel_move_dir_ = MoveDirTag::Forward;
        side_move_dir_ = MoveDirTag::Rightward;
        DEBUG_OUT("#4 SetWheelDir OK(RIGHTWARD);wheelDir= FORWARD;start_steer(degree)=" << start_steer_.Degree()
                                                                                        << ",end_steer=" << end_steer_.Degree());
    }
    DEBUG_WARN_OUT("BezierTrajectory: WheelMoveDir=" << (int)wheel_move_dir_.move_dir_tag_ << ",SideMoveDir=" << (int)side_move_dir_.move_dir_tag_);
}

// Current side_traj  Can the rudder angle be reversed
bool BezierTrajectory::CanSteerReserve(Angle steer_angle) 
{
    bool left = (!steer_angle) > (PI / 2 + PI / 32);
    bool right = (!steer_angle) < ((3 * PI) / 2 - PI / 32);
    return !(left && right);
}

// Vehicle's drive mode on the trajectory
short BezierTrajectory::GetDrivePattern() const 
{
    if (IsShiftMove()) {
        return (short)TagTrajDrivePattern::DRIVE_PATTERN_SHIFT;
    } else {
        return (short)TagTrajDrivePattern::DRIVE_PATTERN_TURN;
    }
}

Bezier* BezierTrajectory::GetBezierObject()
{
    return bezier_;
}

bool BezierTrajectory::IsShiftMove() const
{
    return is_shift_move_;
}

// Get the vehicle's start steer angle
Angle& BezierTrajectory::StartSteerAngle() 
{
    return start_steer_;
}

// Get the vehicle's end steer angle
Angle& BezierTrajectory::EndSteerAngle() 
{
    return end_steer_;
}

Angle& BezierTrajectory::StartHeading() 
{
    return start_heading_;
}

Angle& BezierTrajectory::EndHeading() 
{
    return end_heading_;
}

Point BezierTrajectory::StartPoint() 
{
    return bezier_->StartPoint();
}

Point BezierTrajectory::FirstControlPoint() 
{
    return bezier_->FirstControlPoint();
}

Point BezierTrajectory::SecondControlPoint() 
{
    return bezier_->SecondControlPoint();
}

Point BezierTrajectory::EndPoint() 
{
    return bezier_->EndPoint();
}

//
//   SetCurrentT: Specify the current point on the curve.
//
/**
 * @brief 设置当前时间参数并更新当前轨迹点和曲率
 *
 * 根据给定的速率和时间参数更新当前轨迹点和曲率，并根据车辆移动状态更新车头方向角。
 *
 * @param rate 当前轨迹点的速率
 * @param cur_t 当前轨迹点的时间参数
 * @param update 是否更新轨迹点
 */
void BezierTrajectory::SetCurrentT(float rate, float cur_t, bool update, bool update_rate) 
{   
    if (update_rate) {
        progress_rate_ = rate;
        DEBUG_OUT("set CurrentT:updateRate:fRate=" << rate);
    } else {
        DEBUG_OUT("set CurrentT:can not updateRate:fRate=" << rate);
    }
    
    bezier_->SetCurrentLambda(cur_t, update);
    current_point_ = bezier_->TrajectoryFunction();
    curvature_ = bezier_->CurvatureFunction();
    if (!IsShiftMove()) {
        heading_angle_ = bezier_->TangentFunction();
        heading_angle_ = wheel_move_dir_ == MoveDirTag::Backward ? !heading_angle_ : heading_angle_;
    } else {
        heading_angle_ = shift_heading_; // 车头方向角不变;与轮转向无关
    }
}

//
//   SetProgress: Specify the current point on the curve.
//   FIXME: if dyn locate is locate 1.8;
//   progress[ 0~ 2(m_fRange)];but total bezier is 0~3.8m
//   inputParam:  fRate: vel; fProgress : distance(m)
//   outputParam: find fCurT
//
/**
 * @brief 设置贝塞尔轨迹的相对进度
 *
 * 根据给定的速率和进度，计算当前贝塞尔曲线上的参数t，并更新当前进度。
 *
 * @param rate 速率，表示每单位时间需要移动的贝塞尔曲线长度
 * @param progress 进度，表示已经走过的贝塞尔曲线长度占总长度的比例
 */
void BezierTrajectory::SetRelProgress(float rate, float progress) 
{
    float cur_t = 0.0;
    if (progress > range_) progress = range_;
    if (progress < 0.0f) progress = 0.0f;

    // 是否是停车时，设置进度规划。
    bool is_stop = fabs(rate) < 0.0001f;

    // When the progress is equal to 0.0 or 1.0,
    // there is no need to find the optimal solution
    if (fabs(progress) < 1e-8f) {
        DEBUG_OUT("bezier:Set RelProgress(0):rate=" << rate << ",fProgress=" << progress);
        SetCurrentT(rate, from_ / bezier_->Length());
        return;
    } else if (fabs(progress - range_) < 1e-8f) {
        DEBUG_OUT("bezier:Set RelProgress(1):rate=" << rate << ",fProgress=" << progress);
        SetCurrentT(rate, to_ / bezier_->Length());
        return;
    }
    
    DEBUG_OUT("bezier:Set RelProgress(normal):rate=" << rate << ",fProgress=" << progress);

    // find fCurT start; pso: Particle swarm optimization algorithm
    // Calculating curve parameter t by curve length
    double *lower_t, *upper_t, *max_v;                                        // T的限制值和速度限制值
    double lower_weight = 0.1, upper_weight = 0.5;                            // w= 0.4
    double threshold = 0.005, t_bound = 0.2, position[6][1], velocity[6][1];  // 0.0001
    double c1 = 2, c2 = 2;
    int number_of_particles = NUM_PARTICLES, number_of_variables = NUM_VARIABLES, max_iteration = MAX_ITERATION;
    std::string stopping_criteria("impBest"), pso_type("Classic");
    float estimate_t[4] = {0.0, 0.0, 0.0, 0.0};
    last_plan_t_ = bezier_->GetCurrentT();

    t_bound = fabs(rate) * 0.5;
    t_bound = t_bound < 1e-6f ? 0.005 : t_bound;
    
    // 预先设定目标评估T值,加快收敛迭代
    estimate_t[0] = (progress + from_) / bezier_->Length();
    estimate_t[1] = last_plan_t_ + plan_delta_t_;
    estimate_t[1] = fabs(estimate_t[1]) < 1e-8f ? 0.00012 : estimate_t[1];
    estimate_t[2] = (estimate_t[0] + estimate_t[1]) / 2.0;
    estimate_t[3] = 1.0;

    lower_t = new double[number_of_variables];
    upper_t = new double[number_of_variables];
    max_v = new double[number_of_variables];
    lower_t[0] = last_plan_t_;
    upper_t[0] = last_plan_t_ + t_bound;
    upper_t[0] = upper_t[0] > 1.0 ? 1.0 : upper_t[0];
    max_v[0] = fabs(rate) * 0.1;
    max_v[0] = max_v[0] < 1e-6f ? 0.002 : max_v[0];

    global_bezier = bezier_;
    global_prog_rate = rate;

    // FXIME:速度为0 时会评估失败
    DEBUG_OUT("bezier:set rel progress:"
              << ",esT[0]=" << estimate_t[0]
              << ",esT[1]=" << estimate_t[1]
              << ",esT[2]=" << estimate_t[2]
              << ",esT[3]=" << estimate_t[3]);

    DEBUG_OUT("bezier:set rel progress:"
              << ",m_fLastCurPlanT=" << last_plan_t_
              << ",record diff:m_fPlanDeltaT=" << plan_delta_t_
              << ",TBound=" << t_bound
              << ",lowerT[0]=" << lower_t[0]
              << ",upperT[0]=" << upper_t[0]
              << ",maxV[0]=" << max_v[0]);

    PSOMethod pso_method(number_of_particles, number_of_variables, lower_t, upper_t, max_v,
                          ObjFunByProg, lower_weight, upper_weight, max_iteration,
                          c1, c2, threshold, stopping_criteria, pso_type);

    position[0][0] = estimate_t[0];
    velocity[0][0] = max_v[0] * 0.01;
    position[1][0] = estimate_t[0];
    velocity[1][0] = -max_v[0] * 0.01;
    position[2][0] = estimate_t[1];
    velocity[2][0] = max_v[0] * 0.01;
    position[3][0] = estimate_t[1];
    velocity[3][0] = -max_v[0] * 0.01;
    position[4][0] = estimate_t[2];
    velocity[4][0] = max_v[0] * 0.01;
    position[5][0] = estimate_t[2];
    velocity[5][0] = -max_v[0] * 0.01;
    
    // 预设6个粒子的位置;剩下的6粒子使用随机值
    pso_method.Configure(0, position[0], velocity[0]);
    pso_method.Configure(1, position[1], velocity[1]);
    pso_method.Configure(2, position[2], velocity[2]);
    pso_method.Configure(3, position[3], velocity[3]);
    pso_method.Configure(4, position[4], velocity[4]);
    pso_method.Configure(5, position[5], velocity[5]);

    bool valid = true;
    int ret = pso_method.Compute();
    const double* position_result = pso_method.GetBestPosition();
    double value = pso_method.GetBestValue();
    bool feasible = pso_method.GetBestFeasibility();
    (void)feasible;

    if (ret >= 0 && Particle::GetNumberOfVariables() == NUM_VARIABLES) {
        if (ret >= max_iteration && fabs(value) > threshold) {
            estimate_t[3] = position_result != nullptr ? position_result[0] : 0.0f;
            valid = false;
            DEBUG_ERROR_OUT("#1 error:pso compute failed:" << "nRet=" << ret
                                                           << ",Best fValue=" << value
                                                           << ",maxIteration=" << max_iteration
                                                           << ",threshold=" << threshold
                                                           << ",fEstimateT[3]=" << estimate_t[3]);
        } else {
            if (position_result != nullptr) {
                cur_t = position_result[0];
                valid = true;
                DEBUG_OUT("pso compute ok:" << ",fCurT=" << cur_t);
            } else {
                estimate_t[3] = 0.0f;
                valid = false;
                DEBUG_ERROR_OUT("#2 error:pso compute failed:" << ",EstimateT[3]=" << estimate_t[3]);

            }
        }
    } else {
        estimate_t[3] = position_result != nullptr ? position_result[0] : 0.0f;
        valid = false;
        DEBUG_ERROR_OUT("#3 error:pso compute failed:" << ",EstimateT[3]=" << estimate_t[3]); 
    }
    // if it is inValid so use fEstimateValue[0-3] 's smaller
    if (!valid) {
        float estimate_value[4] = {10.0, 10.0, 10.0, 10.0};
        estimate_value[0] = global_bezier->ComputeRelativeLength(estimate_t[0]) - global_prog_rate * AGV_CTRL_CYCLE;
        estimate_value[1] = global_bezier->ComputeRelativeLength(estimate_t[1]) - global_prog_rate * AGV_CTRL_CYCLE;
        estimate_value[2] = global_bezier->ComputeRelativeLength(estimate_t[2]) - global_prog_rate * AGV_CTRL_CYCLE;
        estimate_value[3] = global_bezier->ComputeRelativeLength(estimate_t[3]) - global_prog_rate * AGV_CTRL_CYCLE;

        int index = 0;
        float optimal_value = estimate_value[0];
        for (int i = 1; i < 4; i++) {
            if (fabs(optimal_value) > fabs(estimate_value[i])) {
                optimal_value = estimate_value[i];
                index = i;
            }
        }
        cur_t = estimate_t[index];

        DEBUG_ERROR_OUT("#4 error:pso compute failed: selected one smaller;"
                        << ",index=" << index
                        << ",fCurT=" << cur_t
                        << ",esT[0]=" << estimate_t[0]
                        << ",esT[1]=" << estimate_t[1]
                        << ",esT[2]=" << estimate_t[2]
                        << ",esT[3]=" << estimate_t[3]);
    }

    if (lower_t != nullptr) {
        delete[] lower_t;
        lower_t = nullptr;
    }
    if (upper_t != nullptr) {
        delete[] upper_t;
        upper_t = nullptr;
    }
    if (max_v != nullptr) {
        delete[] max_v;
        max_v = nullptr;
    }
    global_bezier = nullptr;

    if (is_stop || !valid) {
        // FIXME:当急停时，不进行设置进度时，
        // 设置进度时，必须设置速度，不合理
        progress_rate_ = rate;
        DEBUG_WARN_OUT("warn:setRelProgress can not set current T;" 
            << "isStop=" << is_stop
            << ",bValid=" << valid
            << ",fCurT=" << cur_t
            << ",m_fLastPlanT=" << last_plan_t_
            << ",m_fPlanDeltaT=" << plan_delta_t_
            << ",fRate=" << rate);
    } else {
        plan_delta_t_ = cur_t - last_plan_t_;
        DEBUG_OUT("setRelProgress;can set currentT=" << cur_t << ",m_fLastPlanT=" << last_plan_t_
            << ",m_fPlanDeltaT=" << plan_delta_t_
            << ",fRate=" << rate);
        SetCurrentT(rate, cur_t);
    }
}

//
//   VelocityFun: The vehicle velocity generation function. It will
//   override the virtual function in the base class "CTraj".
//
Velocity& BezierTrajectory::VelocityFun() 
{
    float linear = 0.0f;
    if (IsShiftMove()) {
        linear = progress_rate_;
        velocity_.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? linear : -linear;
        velocity_.angular_ = 0.0f;
        velocity_.vel_x_ = cos(steer_angle_) * velocity_.linear_;
        velocity_.vel_y_ = sin(steer_angle_) * velocity_.linear_;
    } else {
        linear = progress_rate_;
        // 防止曲率过大，导致角速度很大
        float angular = linear * bezier_->CurvatureFunction();
        if (fabs(angular) > 1.5) {
            angular = SIGN(angular) * 0.1f;
            DEBUG_ERROR_OUT("error:bezier curvature is too big;linear=" << linear << ",curvature=" << bezier_->CurvatureFunction());
        }
        velocity_.angular_ = angular;
        velocity_.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? linear : -linear;
        velocity_.vel_x_ = velocity_.linear_;
        velocity_.vel_y_ = 0.0f;
        DEBUG_OUT("bezier:theory:angular=" << velocity_.angular_ << ",linear=" << velocity_.linear_ << ",vel_x=" << velocity_.vel_x_ << ",vel_y=" << velocity_.vel_y_);
    }
    return velocity_;  // Return the vehicle's velocity vector
}

Velocity BezierTrajectory::VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) 
{
    Velocity vel;
    SteeringFun();
    if (IsShiftMove()) {
        vel.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? virtual_velocity : -virtual_velocity;
        vel.vel_x_ = cos(steer_angle_) * vel.linear_;
        vel.vel_y_ = sin(steer_angle_) * vel.linear_;
        vel.angular_ = 0.0f;
        // 曲线平移时，当当前舵角度是0度附近时，会出现问题，改为自由结算
        //  bool zeroAngle = fabs(m_angSteer.NormAngle2()) < 0.0001f;
        //  bool positive = m_angSteer.NormAngle2() > 0.0001f;
        //  steerDir = zeroAngle ? SteerDirAuto : (positive ? SteerDirPositie : SteerDirOpposite);
        steer_direction = (int)SteerDirReport::SteerDirAuto;
    } else {
        steer_direction = (int)SteerDirReport::SteerDirAuto;
        float angular = virtual_velocity * bezier_->CurvatureFunction();
        vel.angular_ = angular;
        vel.linear_ = (wheel_move_dir_ == MoveDirTag::Forward) ? virtual_velocity : -virtual_velocity;
        vel.vel_x_ = vel.linear_;
        vel.vel_y_ = 0.0f;
    }
    DEBUG_WARN_OUT("BezierTrajectory:VelocityForSteerAngle:virtual_velocity=" << virtual_velocity << ",steer_(degree)=" << steer_angle_.Degree() << ",steer_direction=" << steer_direction
                                                                              << ",vel.vel_x=" << vel.vel_x_ << ",vel.vel_y=" << vel.vel_y_ << ",vel.angular=" << vel.angular_);
    return vel;
}

//
//   SteeringFun: The steering angle generation function.
//
Angle& BezierTrajectory::SteeringFun() 
{
    if (!IsShiftMove()) {
        return steer_angle_;
    }
    Angle steer_angle = bezier_->TangentFunction() - shift_heading_;
    steer_angle_ = is_steer_reverse_ ? !steer_angle : steer_angle;
    if (steer_angle_ > PI / 2 + PI / 32 && steer_angle_ < (3 * PI) / 2 - PI / 32) {
        DEBUG_ERROR_OUT("error:BezierTrajectory:steer_ is error;steer_(degree)=" << steer_angle_.Degree()
                                                                                 << ",is_steer_reverse_=" << is_steer_reverse_);
    }
    return steer_angle_;
}

float BezierTrajectory::StartCurvature() 
{
    return start_curvature_;
}

float BezierTrajectory::EndCurvature() 
{
    return end_curvature_;
}

//
//   Get the deviation between the trajectory and the specified posture.
//   FIXME:Need test
/**
 * @brief 将给定的姿态投影到贝塞尔轨迹上，并返回投影数据
 *
 * 该函数将给定的姿态投影到贝塞尔轨迹上，并计算出相关的投影数据，包括投影点、投影角度、切线角度、曲率等。
 *
 * @param posture 要投影的姿态
 * @param data 用于存储投影数据的指针
 * @param init_posture 是否为初始化姿态
 *
 * @return 投影操作是否成功
 */
bool BezierTrajectory::ProjectPosture(Posture& posture, ProjectData* data, bool init_posture) 
{
    if (init_posture) {
        global_prog_rate = PSO_FIND_POS_VEL;
        DEBUG_OUT("first calc locate posture to find near ref point");
    }

    bool result = true, compute_prog = false, valid = true;
    float rel_progress = 0.0, out_end_y = 0;                     // 超出终点的直线距离进度
    USHORT type = (USHORT)ProjectData::ProjectionType::type_inside;
    Point& actual_point = posture.GetPntObject();

    double* lower_t, * upper_t, * max_v;
    double lower_weight = 0.1, upper_weight = 0.5;
    double threshold = 0.01, t_bound = 0.2, position[6][1], velocity[6][1];
    double c1 = 2, c2 = 2;
    int number_of_particles = NUM_PARTICLES, number_of_variables = NUM_VARIABLES, max_iteration = MAX_ITERATION;
    std::string stopping_criteria("impBest"), pso_type("Classic");
    float ref_t = 0.0, estimate_t[4] = {0.0, 0.0, 0.0, 0.0};
    last_ref_t_ = bezier_->GetCurrentT();

    if (last_posture_ == posture) {
        *data = last_project_data_;
        DEBUG_OUT("ProjectPosture(bezier): pst is same; set true");
        return false;
    } else {
        last_posture_ = posture;
    }
    
    // If the actual attitude is not within the curve range,
    // special treatment is required before the beginning of the curve or after the end of the curve
    if (last_ref_t_ < 0.2) {
    	// 在起始点的外侧
        Line start_to_actual(bezier_->StartPoint(), actual_point);
        Angle temp1 = start_to_actual.SlantAngle() - bezier_->StartAngle();
        if (temp1.radian_ > (PI / 2.0) && temp1.radian_ < (3.0 * PI / 2.0)) {
            ref_t = 0.0;
            rel_progress = 0.0f;
            type = (USHORT)ProjectData::ProjectionType::type_outside_start;
            valid = true;
            result = false;
            // 如果首先找到附近的点来计算a t；这是错误的
            DEBUG_OUT("#1 bezier:type_outside_start t=0,rel_prog=0");
        } else {
            type = (USHORT)ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#2 bezier:type_inside,near the start");
        }
    } else if (last_ref_t_ > 0.8) {
    	// 在终止点的外侧
        // FIXME:ln2方向与终端舵角方向相反
        Line end_to_actual(bezier_->EndPoint(), actual_point);
        Angle temp2 = end_to_actual.SlantAngle() - bezier_->EndAngle();
        if (temp2.radian_ >= (PI / 2.0) && temp2.radian_ <= (3.0 * PI / 2.0)) {
        	// angTemp2它是正确的（角度是钝角）
            //  FIXME: CLine ln2(m_pBezier->EndPnt(), pt);EndAng = [pnt[2] to  EndPnt()]
            type = (USHORT)ProjectData::ProjectionType::type_inside;
            DEBUG_OUT("#2 bezier:type_inside near the end");
        } else {
            ref_t = 1.0;
            rel_progress = range_;
            float len_end_to = end_to_actual.Length();
            float add = fabs(len_end_to * cos(temp2));
            out_end_y = range_ + add;
            type = (USHORT)ProjectData::ProjectionType::type_outside_end;
            valid = true;
            result = false;
            DEBUG_OUT("#3 bezier:error: type_outside_end:"
                      << "len_end_to=" << len_end_to
                      << ",add=" << add
                      << ",range_=" << range_
                      << ",out_end_y=" << out_end_y);
        }
    }
    // #1 try to find fRefT
    // uType: 0 normal effective rRadiusange 
    if (type == (USHORT)ProjectData::ProjectionType::type_inside) {
        float current_t = bezier_->GetCurrentT();

        t_bound = fabs(global_prog_rate) * 0.6;
        t_bound = t_bound < 1e-6f ? 0.01 : t_bound;

        estimate_t[0] = current_t;
        estimate_t[1] = current_t + ref_delta_t_;
        estimate_t[1] = fabs(estimate_t[1]) < 1e-8f ? 0.00012 : estimate_t[1];
        estimate_t[2] = last_plan_t_;
        estimate_t[3] = 1.0;

        lower_t = new double[number_of_variables];
        upper_t = new double[number_of_variables];
        max_v = new double[number_of_variables];
        lower_t[0] = (current_t - t_bound < 0.0) ? 0.0 : (current_t - t_bound);
        upper_t[0] = (current_t + t_bound > 1.0) ? 1.0 : (current_t + t_bound);
        max_v[0] = (fabs(global_prog_rate) * 0.1 < 1e-6f) ? 0.002 : (fabs(global_prog_rate) * 0.1);

        global_bezier = bezier_;
        global_posture = posture;

        DEBUG_OUT("bezier:projectPosture:"
                  << ",esT[0]=" << estimate_t[0]
                  << ",esT[1]=" << estimate_t[1]
                  << ",esT[2]=" << estimate_t[2]
                  << ",esT[3]=" << estimate_t[3]);

        DEBUG_OUT("bezier:projectPosture:"
                  << ",v=" << global_prog_rate
                  << ",fCurT=" << current_t
                  << ",referDeltaT=" << ref_delta_t_
                  << ",fLastPlanT=" << last_plan_t_
                  << ",TBound=" << t_bound
                  << ",lowerT[0]=" << lower_t[0]
                  << ",upperT[0]=" << upper_t[0]
                  << ",maxV[0]=" << max_v[0]);

        PSOMethod pso_method(number_of_particles, number_of_variables, lower_t, upper_t, max_v,
                              ObjFunByPosture, lower_weight, upper_weight, max_iteration,
                              c1, c2, threshold, stopping_criteria, pso_type);

        position[0][0] = estimate_t[0];
        velocity[0][0] = max_v[0] * 0.01;
        position[1][0] = estimate_t[0];
        velocity[1][0] = -max_v[0] * 0.01;
        position[2][0] = estimate_t[1];
        velocity[2][0] = max_v[0] * 0.01;
        position[3][0] = estimate_t[1];
        velocity[3][0] = -max_v[0] * 0.01;
        position[4][0] = estimate_t[2];
        velocity[4][0] = max_v[0] * 0.01;
        position[5][0] = estimate_t[2];
        velocity[5][0] = -max_v[0] * 0.01;

        pso_method.Configure(0, position[0], velocity[0]);
        pso_method.Configure(1, position[1], velocity[1]);
        pso_method.Configure(2, position[2], velocity[2]);
        pso_method.Configure(3, position[3], velocity[3]);
        pso_method.Configure(4, position[4], velocity[4]);
        pso_method.Configure(5, position[5], velocity[5]);

        int ret = pso_method.Compute();
        const double* position_result = pso_method.GetBestPosition();
        double value = pso_method.GetBestValue();
        bool feasible = pso_method.GetBestFeasibility();
        (void)feasible;

        if (ret >= 0 && Particle::GetNumberOfVariables() == NUM_VARIABLES) {
            if (ret >= max_iteration && fabs(value) > threshold) {
                estimate_t[3] = position_result != nullptr ? position_result[0] : 0.0f;
                valid = false;
                DEBUG_ERROR_OUT("#1 b:project:error:pso compute failed:"
                                << "nRet=" << ret
                                << ",value=" << value
                                << ",threshold=" << threshold
                                << ",est[3]=" << estimate_t[3]);
            } else {
                if (position_result != nullptr) {
                    ref_t = position_result[0];
                    valid = true;
                    DEBUG_OUT("#1 b:project ok:t=" << ref_t);

                } else {
                    estimate_t[3] = 0.0f;
                    valid = false;
                    DEBUG_OUT("#2 b:project failed;");
                }
            }
        } else {
            estimate_t[3] = position_result != nullptr ? position_result[0] : 0.0f;
            valid = false;
            DEBUG_ERROR_OUT("#3 b:project failed;" << ",est[3]=" << estimate_t[3]);
        }

        if (!valid) {
            float estimate_value[4] = {10.0, 10.0, 10.0, 10.0};
            estimate_value[0] = global_bezier->ComputeReferencePoint(estimate_t[0], global_posture.x_, global_posture.y_);
            estimate_value[1] = global_bezier->ComputeReferencePoint(estimate_t[1], global_posture.x_, global_posture.y_);
            estimate_value[2] = global_bezier->ComputeReferencePoint(estimate_t[2], global_posture.x_, global_posture.y_);
            estimate_value[3] = global_bezier->ComputeReferencePoint(estimate_t[3], global_posture.x_, global_posture.y_);

            int index = 0;
            float optimal_value = estimate_value[0];
            for (int i = 1; i < 4; i++) {
                if (fabs(optimal_value) > fabs(estimate_value[i])) {
                    optimal_value = estimate_value[i];
                    index = i;
                }
            }
            ref_t = estimate_t[index];
            valid = fabs(optimal_value) < threshold * 80.0 ? true : false;
            DEBUG_ERROR_OUT("#4 b:project: error;selected one smaller;"
                            << ",index=" << index
                            << ",fRefT=" << ref_t
                            << ",valid=" << valid
                            << ",threshold=" << threshold
                            << ",esT[0]=" << estimate_value[0]
                            << ",esT[1]=" << estimate_value[1]
                            << ",esT[2]=" << estimate_value[2]
                            << ",esT[3]=" << estimate_value[3]);
        }

        if (lower_t != nullptr) {
            delete[] lower_t;
            lower_t = nullptr;
        }
        if (upper_t != nullptr) {
            delete[] upper_t;
            upper_t = nullptr;
        }
        if (max_v != nullptr) {
            delete[] max_v;
            max_v = nullptr;
        }
        global_bezier = nullptr;
        if (!compute_prog) {
            ref_delta_t_ = ref_t - last_ref_t_;
        }
    }

    Point ref_point;
    Angle ref_angle;
    if (valid) {
        if (compute_prog) {
            // FIXME：评估速度设置0 不合理
            SetCurrentT(0.0f, ref_t, false);
            DEBUG_OUT("bezier:projectData: bComputeProg true;set fRate=0;fRefT=" << ref_t);
        } else {
            SetCurrentT(0.0f, ref_t, init_posture || reload_area_flag_, false);
            DEBUG_OUT("bezier:projectData: computeProg false;set fRate=0;initPst=" << init_posture
                                                                                   << ",flag=" << reload_area_flag_ << ",fRefT=" << ref_t);
        }

        if (ref_t >= 0.0 && ref_t <= 1.0) {
            rel_progress = bezier_->CurveLength() - from_;
            rel_progress = rel_progress < 0.0f ? 0.0f : rel_progress;
        }

        ref_point = TrajFun();
        ref_angle = HeadingFun();
        Angle angle_diff = posture.GetAngle() - ref_angle;
        data->theta_ = angle_diff.radian_;
        data->type_ = (ProjectData::ProjectionType)type;
        data->y_ = rel_progress;
        data->relative_progress_ = rel_progress;
        data->tangent_ = ref_angle.radian_;
        data->curvature_ = CurvatureFun();

        if (ref_point != actual_point) {
            Line line(ref_point, actual_point);
            data->x_ = line.Length();
            Angle temp_angle = line.SlantAngle() - ref_angle;
            if (temp_angle.Degree() < 180.0f) {
                data->x_ = -data->x_;
            }
            Angle dev_angle;
            if (type == (USHORT)ProjectData::ProjectionType::type_outside_start) {
                dev_angle = !ref_angle - line.SlantAngle();
                data->x_ = data->x_ * fabsf(sin(dev_angle.radian_));
                data->y_ = 0;
            } else if (type == (USHORT)ProjectData::ProjectionType::type_outside_end) {
                dev_angle = ref_angle - line.SlantAngle();
                data->x_ = data->x_ * fabsf(sin(dev_angle.radian_));
                data->y_ = out_end_y;
            }
        } else {
            data->x_ = 0;
        }
    } else {
        ref_angle = HeadingFun();
        data->theta_ = 0.0;
        data->type_ = ProjectData::ProjectionType::type_inside;
        rel_progress = bezier_->CurveLength() - from_;
        rel_progress = rel_progress < 0.0f ? 0.0f : rel_progress;
        data->relative_progress_ = rel_progress;
        data->y_ = rel_progress;
        data->tangent_ = ref_angle.radian_;
        data->curvature_ = CurvatureFun();
        data->x_ = 0.0;
        result = false;
        DEBUG_OUT("bezier:projectData: failed");
    }
    
    last_project_data_ = *data;
    if (init_posture) {
        global_prog_rate = 0;
    }
    return result;
}

// float CBezierTraj::GetProfileArea(float fLinearArea)
// {
//     return fLinearArea;
// }
float BezierTrajectory::GetProfileValue(float linear_velocity) 
{
    return linear_velocity;
}

float BezierTrajectory::GetProfileSlope(float acc) 
{
    return acc;
}

float BezierTrajectory::GetLinearVel(float value) 
{
    return value;
}

float BezierTrajectory::Radius() 
{
    return bezier_->Radius();
}

float BezierTrajectory::CurrentRadius() 
{
    return bezier_->CurrentRadius();
}

float BezierTrajectory::GetTotalLength() 
{
    return bezier_ ? bezier_->Length() : -1.0f;
}

//
float BezierTrajectory::GetCurveLength() 
{
    return bezier_ ? bezier_->CurveLength() : -1.0f;
}

//
float BezierTrajectory::GetCurT() 
{
    return bezier_ ? bezier_->GetCurrentT() : -1.0f;
}

//
float BezierTrajectory::GetLastT() 
{
    return bezier_ ? bezier_->GetLastT() : -1.0f;
}

//
float BezierTrajectory::GetDeltaT() 
{
    return bezier_ ? bezier_->GetDeltaT() : -1.0f;
}

//
// try to update cur T(Progress) by real feedback pos(x,y,theta)
//
/**
 * @brief 设置重新加载区域标志
 *
 * 设置重新加载区域标志，用于控制是否需要重新加载区域数据。
 *
 * @param flag 重新加载区域标志
 *              - true：需要重新加载区域数据
 *              - false：不需要重新加载区域数据
 */
void BezierTrajectory::SetReloadAreaFlag(bool flag) 
{
    reload_area_flag_ = flag;
    DEBUG_OUT("BezierTraj:SetReloadAreaFlag:bFlag=" << flag);
}

/**
 * @brief 计算当前航向角与贝塞尔曲线切线的夹角
 *
 * 计算当前航向角与贝塞尔曲线在该点的切线之间的夹角，并返回该夹角。
 *
 * @return 返回计算得到的夹角，类型为Angle。
 */
Angle BezierTrajectory::HeadToTangent() 
{
    Angle angle = heading_angle_ - bezier_->TangentFunction();
    DEBUG_OUT("BezierTrajectory:HeadToTangent:heading(degree)=" << heading_angle_.Degree() << ",tangentAngle(degree)=" << bezier_->TangentFunction().Degree()
                                                                << ",angle(degree)=" << angle.Degree() << ",is_shift_move_=" << is_shift_move_);
    return angle;
}
